#ifndef SROS_LOCATION_UPDATE_IMPL_HPP
#define SROS_LOCATION_UPDATE_IMPL_HPP

#include <commons/impl/noncopyable.hpp>

#include <Eigen/Dense>

#include <mutex>
#include <memory>

namespace sros::location
{
    template<typename HT, typename RT, typename ZT, typename PT>
    class update_impl : public impl::noncopyable
    {
    public:
        struct Param
        {
            HT H_;  // 观测矩阵
            RT R_;  // 观测噪声
            ZT Z_;  // 观测数据
            PT P_;  // 预测模型指针
        };
        
        update_impl() : impl::noncopyable()
        {
            init_();
        }

        virtual ~update_impl() = default;

        void set_param(const Param &param)
        {
            std::lock_guard<std::mutex> lck(mtx_);
            param_ = std::make_shared<Param>(param);
        }

        void set_R(const RT &r)
        {
            std::lock_guard<std::mutex> lck(mtx_);
            param_->R_ = r;
        }

        void set_Z(const ZT &z)
        {
            std::lock_guard<std::mutex> lck(mtx_);
            param_->Z_ = z;
        }

        void set_P(PT p)
        {
            std::lock_guard<std::mutex> lck(mtx_);
            param_->P_ = p;
        }

        virtual void update() { }

    protected:
        virtual void init_()
        {
            param_ = std::make_shared<Param>();
        }

    protected:
        std::mutex              mtx_;
        std::shared_ptr<Param>  param_;
    };
}

#endif